#include "MeasurE616AlgBackD1ToD8Circle.h"

MeasurE616AlgBackD1toD8Circle::MeasurE616AlgBackD1toD8Circle(int i):MeasurE616AlgBase(i)
{

}

void MeasurE616AlgBackD1toD8Circle::operator()(const cv::Mat &srcImgTemp, int id,std::map<string, float> &measRes, int fixtureId)
{
    //背光图小半圆边缘提取  // CDL back图    \paras616\back\ cdL.xml
//    int NewCircleEdge(cv::Mat srcimg, const vector<int> paras, vector<cv::Point>& contours);

    qDebug() << "E616 alg back d1 to d8 circle start";
    if(!srcImgTemp.data) {
        qDebug() << "input empty image";
        Q_ASSERT(srcImgTemp.data);
        return;
    }
    cv::Mat srcImg = srcImgTemp.clone();

    std::vector<cv::Point> resptVec;
    MeasurE616EdgeDetect obj;
    for(auto i=9;i<17;i++)
    {
        std::vector<int> tparas;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-9,j));
        }
        //paras.push_back(tparas);
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<cv::Point> contours;
        obj.NewCircleEdge(imgt,tparas,contours);
        ImageCoordinateTransformation(contours,rangVec);

//#ifdef MEASUR_E616_DRAW_IMG
//        static int dCircleId = 0;
//        std::string imgName = ("circle" + to_string(dCircleId));
//        imgName += ".jpg";

//        cv::Mat srcImg = srcImgTemp.clone();
//        for(int i = 0; i < contours.size(); i++) {
//            cv::circle(srcImg, contours[i], 2, 160, -1);
//        }
//        imwrite(imgName, srcImg);

//        dCircleId++;
//#endif
        std::unique_lock<std::mutex> lock(*MeasurE616AlgBase::mtxptrVec[id]);
        while (!MeasurE616AlgBase::flagVec[id])
        {
            (*MeasurE616AlgBase::cvarptrVec[id]).wait(lock);
        }
        for(auto &elem:contours)
        {
            cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
//            CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
            CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
            resptVec.push_back(ptsOut);
        }
    }
    std::vector<float> cirres;
    cirres=obj.FitCircle(resptVec);

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";
    measRes.insert(std::make_pair(string("FAI74-D-X-distance_pixel"), -cirres[0]));
    float fai74dx = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI74-D-X-distance")], -cirres[0]);
    measRes.insert(std::make_pair(string("FAI74-D-X-distance_real"), fai74dx));

    measRes.insert(std::make_pair(string("FAI74-D-Y-distance_pixel"), -cirres[1]));
    float fai74dy = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI74-D-Y-distance")], -cirres[1]);
    measRes.insert(std::make_pair(string("FAI74-D-Y-distance_real"), fai74dy));

    measRes.insert(std::make_pair(string("FAI73-D-diam_pixel"), cirres[2]*2));
    float fai73dd = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI73-D-diam")], cirres[2]*2);
    measRes.insert(std::make_pair(string("FAI73-D-diam_real"), fai73dd));

//    float fai74pos=obj.GetPosition(cv::Point2f(-18.33,-18.164),cv::Point2f(fai74dx,fai74dy));
//    measRes.insert(std::make_pair(string("FAI74-D-position"), fai74pos));

    cv::Point2f stdposFai74(-18.33, -18.164);

    float fai74PosPixelX = CalcPixelAccordExpectDist(m_phypixpara[prefix + std::string("FAI74-D-X-distance")],
            m_standardValMap["FAI74-D-X-distance"]);
    float fai74PosPixelY = CalcPixelAccordExpectDist(m_phypixpara[prefix + std::string("FAI74-D-Y-distance")],
            m_standardValMap["FAI74-D-Y-distance"]);

    double fai74PosPixel = obj.GetPosition(cv::Point2f(-cirres[0], -cirres[1]), cv::Point2f(fai74PosPixelX, fai74PosPixelY));
    measRes.insert(std::make_pair(string("FAI74-D-position_pixel"), fai74PosPixel));
    float fai74PosDis = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI74-D-position")], fai74PosPixel);
    measRes.insert(std::make_pair(string("FAI74-D-position_real"), fai74PosDis));

    qDebug() << "E616 alg back d1 to d8 circle end";

}
